#include "WProgram.h"

void setup();
void loop();
void blink(int whatPin, int howManyTimes, int milliSecs);
int motor1Pin = 3;    // H-bridge leg 1 
int motor2Pin = 4;    // H-bridge leg 2
int motor3Pin = 5;
int motor4Pin = 6;
int speedPin1 = 9;     // H-bridge enable pin 
int speedPin2 = 10;

int ledPin = 13;      //LED 

void setup() {

  // set all the other pins you're using as outputs:
  pinMode(motor1Pin, OUTPUT); 
  pinMode(motor2Pin, OUTPUT);
  pinMode(motor3Pin, OUTPUT);
  pinMode(motor4Pin, OUTPUT);
  pinMode(speedPin1, OUTPUT);
  pinMode(speedPin2, OUTPUT);
  pinMode(ledPin, OUTPUT);

  // set speedPin1 high so that motor can turn on:
  digitalWrite(speedPin1, HIGH); 
  digitalWrite(speedPin2, HIGH);
  
  // blink the LED 3 times. This should happen only once.
  // if you see the LED blink three times, it means that the module
  // reset itself,. probably because the motor caused a brownout
  // or a short.
  blink(ledPin, 3, 100);
}

void loop() {
    // function test
    digitalWrite(motor1Pin, LOW);
    digitalWrite(motor2Pin, LOW);
    digitalWrite(motor3Pin, LOW);
    digitalWrite(motor4Pin, LOW);
    
    delay(1000);
    digitalWrite(motor1Pin, LOW);   // set leg 1 of the H-bridge low
    digitalWrite(motor2Pin, HIGH);  // set leg 2 of the H-bridge high
    
    delay(1000);
    digitalWrite(motor1Pin, HIGH);  // set leg 1 of the H-bridge high
    digitalWrite(motor2Pin, LOW);   // set leg 2 of the H-bridge low
    
    delay(1000);
    digitalWrite(motor1Pin, LOW);
    digitalWrite(motor2Pin, LOW);
    
    delay(1000);
    digitalWrite(motor3Pin, LOW);   // set leg 1 of the H-bridge low
    digitalWrite(motor4Pin, HIGH);  // set leg 2 of the H-bridge high

    delay(1000);
    digitalWrite(motor3Pin, LOW);  // set leg 1 of the H-bridge high
    digitalWrite(motor4Pin, LOW);   // set leg 2 of the H-bridge low
    
    delay(1000);
    digitalWrite(motor3Pin, HIGH);  // set leg 1 of the H-bridge high
    digitalWrite(motor4Pin, LOW);   // set leg 2 of the H-bridge low
    
    delay(1000);
}

/*
  blinks an LED
 */
void blink(int whatPin, int howManyTimes, int milliSecs) {
  int i = 0;
  for ( i = 0; i < howManyTimes; i++) {
    digitalWrite(whatPin, HIGH);
    delay(milliSecs/2);
    digitalWrite(whatPin, LOW);
    delay(milliSecs/2);
  }
}


int main(void)
{
	init();

	setup();
    
	for (;;)
		loop();
        
	return 0;
}

